#pragma once

#include "vio/front_end/frame.h"
#include "vio/tools/vision_utils.h"
#include "vio/vio_common.h"

namespace vio {

// implementation of various feature detectors
namespace feature_detection {
// temporary container used for corner detection. features are initialized from
// these.
struct Corner {
  int x;        // x-coordinate of corner in the image
  int y;        // y-coordinate of corner in the image
  int level;    // pyramid level of the corner
  float score;  // shi-tomasi score of the corner
  float angle;  // for gradient-features: dominant gradient angle
  Corner(int x, int y, float score, int level, float angle)
      : x(x), y(y), level(level), score(score), angle(angle) {}
};
typedef std::vector<Corner> Corners;

// all detectors should derive from this abstract class
class AbstractDetector {
 public:
  typedef std::shared_ptr<AbstractDetector> Ptr;
  AbstractDetector(const int img_width,
                   const int img_height,
                   const int cell_size,
                   const int n_pyr_levels);

  virtual ~AbstractDetector() {}

  virtual void Detect(Frame::Ptr frame,
                      const ImgPyr& img_pyr,
                      const double detection_threshold,
                      Features& fts) = 0;

  // flag for the grid cell as occupied
  void SetGridOccupancy(const Eigen::Vector2d& px);

  // set grid cells of existing features as occupied
  void SetExistingFeatures(const Features& fts);

 protected:
  static const int border_ = 8;  // no feature should be within 8px of border
  const int cell_size_;
  const int n_pyr_levels_;
  const int grid_n_cols_;
  const int grid_n_rows_;
  std::vector<bool> grid_occupancy_;

  void ResetGrid();

  inline int GetCellIndex(int x, int y, int level) {
    const int scale = (1 << level);
    return (scale * y) / cell_size_ * grid_n_cols_ + (scale * x) / cell_size_;
  }
};

class FastDetector : public AbstractDetector {
 public:
  typedef std::shared_ptr<FastDetector> Ptr;
  FastDetector(const int img_width,
               const int img_height,
               const int cell_size,
               const int n_pyr_levels);

  virtual ~FastDetector() {}

  virtual void Detect(Frame::Ptr frame,
                      const ImgPyr& img_pyr,
                      const double detection_threshold,
                      Features& fts);
};
}  // namespace feature_detection
}  // namespace vio
